#!/usr/bin/env python3
"""
USV PID调试数据分析和可视化脚本
用于分析CSV日志文件中的PID控制性能
"""

import pandas as pd
import matplotlib.pyplot as plt
import numpy as np
import argparse
import os
import glob
from datetime import datetime
import seaborn as sns
import re

# 设置中文字体支持
plt.rcParams["font.sans-serif"]=["SimHei"] #设置字体
plt.rcParams["axes.unicode_minus"]=False #该语句解决图像中的“-”负号的乱码问题

# 默认CSV文件搜索路径
DEFAULT_CSV_PATH = "/home/ubuntu/ros2_boat_ws"

def find_csv_files(search_path, pattern="usv_pid_debug_*.csv"):
    """在指定路径下查找CSV文件"""
    search_pattern = os.path.join(search_path, pattern)
    csv_files = glob.glob(search_pattern)
    return sorted(csv_files)

def find_latest_csv(search_path, pattern="usv_pid_debug_*.csv"):
    """查找最新的CSV文件"""
    csv_files = find_csv_files(search_path, pattern)
    if not csv_files:
        return None
    
    # 根据文件修改时间排序，返回最新的
    csv_files.sort(key=lambda x: os.path.getmtime(x), reverse=True)
    return csv_files[0]

def find_csv_by_name(search_path, filename):
    """根据文件名查找CSV文件"""
    # 如果是完整路径，直接返回
    if os.path.isabs(filename) and os.path.exists(filename):
        return filename
    
    # 如果只是文件名，在搜索路径中查找
    if not filename.endswith('.csv'):
        filename += '.csv'
    
    full_path = os.path.join(search_path, filename)
    if os.path.exists(full_path):
        return full_path
    
    # 尝试模糊匹配（包含时间戳的文件）
    if not ('_' in filename and any(char.isdigit() for char in filename)):
        # 如果文件名不包含时间戳，尝试查找包含该名称的文件
        base_name = filename.replace('.csv', '')
        pattern = f"{base_name}_*.csv"
        matching_files = find_csv_files(search_path, pattern)
        if matching_files:
            # 返回最新的匹配文件
            matching_files.sort(key=lambda x: os.path.getmtime(x), reverse=True)
            return matching_files[0]
    
    return None

def get_csv_file_path(args):
    """根据命令行参数确定CSV文件路径"""
    if hasattr(args, 'csv_file') and args.csv_file:
        # 指定了文件路径或文件名
        csv_path = find_csv_by_name(DEFAULT_CSV_PATH, args.csv_file)
        if csv_path:
            return csv_path
        else:
            print(f"错误: 找不到文件 {args.csv_file}")
            print(f"搜索路径: {DEFAULT_CSV_PATH}")
            # 列出可用的CSV文件
            available_files = find_csv_files(DEFAULT_CSV_PATH)
            if available_files:
                print("\n可用的CSV文件:")
                for i, file in enumerate(available_files[-10:], 1):  # 只显示最新的10个
                    file_time = datetime.fromtimestamp(os.path.getmtime(file))
                    print(f"  {i}. {os.path.basename(file)} (修改时间: {file_time.strftime('%Y-%m-%d %H:%M:%S')})")
            return None
    else:
        # 未指定文件，查找最新的
        latest_csv = find_latest_csv(DEFAULT_CSV_PATH)
        if latest_csv:
            print(f"自动选择最新的CSV文件: {os.path.basename(latest_csv)}")
            file_time = datetime.fromtimestamp(os.path.getmtime(latest_csv))
            print(f"文件修改时间: {file_time.strftime('%Y-%m-%d %H:%M:%S')}")
            return latest_csv
        else:
            print(f"错误: 在路径 {DEFAULT_CSV_PATH} 下找不到任何CSV文件")
            print("请确保:")
            print("1. CSV文件保存路径正确")
            print("2. 文件名匹配模式 'usv_pid_debug_*.csv'")
            print("3. 已经运行过USV控制器并生成了日志文件")
            return None

def load_and_process_data(csv_file):
    """加载和预处理CSV数据"""
    try:
        df = pd.read_csv(csv_file)
        print(f"成功加载数据文件: {os.path.basename(csv_file)}")
        print(f"完整路径: {csv_file}")
        print(f"数据形状: {df.shape}")
        
        if len(df) == 0:
            print("警告: CSV文件为空")
            return None
            
        print(f"时间范围: {(df['timestamp'].max() - df['timestamp'].min()) / 1000:.2f} 秒")
        
        # 转换时间戳为相对时间（秒）
        df['time_sec'] = (df['timestamp'] - df['timestamp'].min()) / 1000.0
        
        # 计算一些派生指标
        df['heading_abs_error'] = df['heading_error'].abs()
        df['speed_abs_error'] = df['speed_error'].abs()
        df['total_heading_output'] = df['heading_p'] + df['heading_i'] + df['heading_d']
        df['total_speed_output'] = df['speed_p'] + df['speed_i'] + df['speed_d']
        
        return df
    except Exception as e:
        print(f"加载数据文件失败: {e}")
        return None

def list_available_files(search_path):
    """列出可用的CSV文件"""
    csv_files = find_csv_files(search_path)
    if csv_files:
        print(f"\n在路径 {search_path} 下找到的CSV文件:")
        for i, file in enumerate(csv_files, 1):
            file_time = datetime.fromtimestamp(os.path.getmtime(file))
            file_size = os.path.getsize(file) / 1024  # KB
            print(f"  {i:2d}. {os.path.basename(file)}")
            print(f"      修改时间: {file_time.strftime('%Y-%m-%d %H:%M:%S')}")
            print(f"      文件大小: {file_size:.1f} KB")
            print()
    else:
        print(f"在路径 {search_path} 下没有找到任何CSV文件")

def plot_trajectory(df, save_path=None):
    """绘制轨迹图"""
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6))
    
    # 地理轨迹
    scatter = ax1.scatter(df['current_lon'], df['current_lat'], 
                         c=df['time_sec'], cmap='viridis', s=10)
    ax1.set_xlabel('经度')
    ax1.set_ylabel('纬度')
    ax1.set_title('USV轨迹图')
    ax1.grid(True, alpha=0.3)
    plt.colorbar(scatter, ax=ax1, label='时间 (秒)')
    
    # 航点进度
    ax2.plot(df['time_sec'], df['distance_to_target'], 'b-', linewidth=2)
    ax2.set_xlabel('时间 (秒)')
    ax2.set_ylabel('到目标距离 (米)')
    ax2.set_title('到目标点距离')
    ax2.grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_trajectory.png", dpi=300, bbox_inches='tight')
    plt.show()

def plot_heading_control(df, save_path=None):
    """绘制航向控制分析图"""
    fig, axes = plt.subplots(2, 2, figsize=(15, 10))
    
    # 航向跟踪
    axes[0,0].plot(df['time_sec'], df['current_heading'], 'b-', label='当前航向', linewidth=2)
    axes[0,0].plot(df['time_sec'], df['target_heading'], 'r--', label='目标航向', linewidth=2)
    axes[0,0].set_ylabel('航向角 (度)')
    axes[0,0].set_title('航向跟踪性能')
    axes[0,0].legend()
    axes[0,0].grid(True, alpha=0.3)
    
    # 航向误差
    axes[0,1].plot(df['time_sec'], df['heading_error'], 'r-', linewidth=2)
    axes[0,1].axhline(y=0, color='k', linestyle='-', alpha=0.3)
    axes[0,1].set_ylabel('航向误差 (度)')
    axes[0,1].set_title('航向误差')
    axes[0,1].grid(True, alpha=0.3)
    
    # PID组件分析
    axes[1,0].plot(df['time_sec'], df['heading_p'], 'g-', label='比例项 (P)', linewidth=2)
    axes[1,0].plot(df['time_sec'], df['heading_i'], 'b-', label='积分项 (I)', linewidth=2)
    axes[1,0].plot(df['time_sec'], df['heading_d'], 'r-', label='微分项 (D)', linewidth=2)
    axes[1,0].set_xlabel('时间 (秒)')
    axes[1,0].set_ylabel('PID组件值')
    axes[1,0].set_title('航向PID组件分析')
    axes[1,0].legend()
    axes[1,0].grid(True, alpha=0.3)
    
    # 控制输出
    axes[1,1].plot(df['time_sec'], df['heading_output'], 'purple', linewidth=2, label='总输出')
    axes[1,1].plot(df['time_sec'], df['steer_l'], 'orange', linewidth=2, label='舵角输出')
    axes[1,1].set_xlabel('时间 (秒)')
    axes[1,1].set_ylabel('控制输出')
    axes[1,1].set_title('航向控制输出')
    axes[1,1].legend()
    axes[1,1].grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_heading_control.png", dpi=300, bbox_inches='tight')
    plt.show()

def plot_speed_control(df, save_path=None):
    """绘制速度控制分析图"""
    fig, axes = plt.subplots(2, 2, figsize=(15, 10))
    
    # 速度跟踪
    axes[0,0].plot(df['time_sec'], df['current_speed'], 'b-', label='当前速度', linewidth=2)
    axes[0,0].plot(df['time_sec'], df['target_speed'], 'r--', label='目标速度', linewidth=2)
    axes[0,0].set_ylabel('速度 (m/s)')
    axes[0,0].set_title('速度跟踪性能')
    axes[0,0].legend()
    axes[0,0].grid(True, alpha=0.3)
    
    # 速度误差
    axes[0,1].plot(df['time_sec'], df['speed_error'], 'r-', linewidth=2)
    axes[0,1].axhline(y=0, color='k', linestyle='-', alpha=0.3)
    axes[0,1].set_ylabel('速度误差 (m/s)')
    axes[0,1].set_title('速度误差')
    axes[0,1].grid(True, alpha=0.3)
    
    # PID组件分析
    axes[1,0].plot(df['time_sec'], df['speed_p'], 'g-', label='比例项 (P)', linewidth=2)
    axes[1,0].plot(df['time_sec'], df['speed_i'], 'b-', label='积分项 (I)', linewidth=2)
    axes[1,0].plot(df['time_sec'], df['speed_d'], 'r-', label='微分项 (D)', linewidth=2)
    axes[1,0].set_xlabel('时间 (秒)')
    axes[1,0].set_ylabel('PID组件值')
    axes[1,0].set_title('速度PID组件分析')
    axes[1,0].legend()
    axes[1,0].grid(True, alpha=0.3)
    
    # 电机输出
    axes[1,1].plot(df['time_sec'], df['motor_l'], 'blue', linewidth=2, label='左电机')
    axes[1,1].plot(df['time_sec'], df['motor_r'], 'red', linewidth=2, label='右电机')
    axes[1,1].set_xlabel('时间 (秒)')
    axes[1,1].set_ylabel('电机输出 (RPM)')
    axes[1,1].set_title('电机输出')
    axes[1,1].legend()
    axes[1,1].grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_speed_control.png", dpi=300, bbox_inches='tight')
    plt.show()

def plot_pid_performance_summary(df, save_path=None):
    """绘制PID性能总结图"""
    fig, axes = plt.subplots(2, 3, figsize=(18, 10))
    
    # 误差统计分析
    axes[0,0].hist(df['heading_abs_error'], bins=50, alpha=0.7, color='blue', edgecolor='black')
    axes[0,0].set_xlabel('航向绝对误差 (度)')
    axes[0,0].set_ylabel('频次')
    axes[0,0].set_title('航向误差分布')
    axes[0,0].grid(True, alpha=0.3)
    
    axes[0,1].hist(df['speed_abs_error'], bins=50, alpha=0.7, color='green', edgecolor='black')
    axes[0,1].set_xlabel('速度绝对误差 (m/s)')
    axes[0,1].set_ylabel('频次')
    axes[0,1].set_title('速度误差分布')
    axes[0,1].grid(True, alpha=0.3)
    
    # 误差随时间变化
    axes[0,2].plot(df['time_sec'], df['heading_abs_error'], 'b-', alpha=0.7, label='航向误差')
    axes[0,2].plot(df['time_sec'], df['speed_abs_error'], 'g-', alpha=0.7, label='速度误差')
    axes[0,2].set_xlabel('时间 (秒)')
    axes[0,2].set_ylabel('绝对误差')
    axes[0,2].set_title('误差随时间变化')
    axes[0,2].legend()
    axes[0,2].grid(True, alpha=0.3)
    
    # 控制稳定性分析
    window_size = min(50, len(df) // 10)
    if window_size > 1:
        df['heading_error_std'] = df['heading_abs_error'].rolling(window=window_size).std()
        df['speed_error_std'] = df['speed_abs_error'].rolling(window=window_size).std()
        
        axes[1,0].plot(df['time_sec'], df['heading_error_std'], 'purple', linewidth=2)
        axes[1,0].set_xlabel('时间 (秒)')
        axes[1,0].set_ylabel('航向误差标准差')
        axes[1,0].set_title('航向控制稳定性')
        axes[1,0].grid(True, alpha=0.3)
        
        axes[1,1].plot(df['time_sec'], df['speed_error_std'], 'orange', linewidth=2)
        axes[1,1].set_xlabel('时间 (秒)')
        axes[1,1].set_ylabel('速度误差标准差')
        axes[1,1].set_title('速度控制稳定性')
        axes[1,1].grid(True, alpha=0.3)
    
    # 任务进度
    axes[1,2].plot(df['time_sec'], df['waypoint_index'], 'mo-', markersize=4, linewidth=2)
    axes[1,2].set_xlabel('时间 (秒)')
    axes[1,2].set_ylabel('当前航点索引')
    axes[1,2].set_title('任务进度')
    axes[1,2].grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_performance_summary.png", dpi=300, bbox_inches='tight')
    plt.show()

def generate_statistics_report(df):
    """生成统计报告"""
    print("\n" + "="*60)
    print("USV PID控制性能统计报告")
    print("="*60)
    
    # 基本信息
    total_time = (df['timestamp'].max() - df['timestamp'].min()) / 1000.0
    total_waypoints = df['total_waypoints'].iloc[0] if len(df) > 0 else 0
    completed_waypoints = df['waypoint_index'].max()
    
    print(f"总测试时间: {total_time:.2f} 秒")
    print(f"总航点数: {total_waypoints}")
    print(f"完成航点数: {completed_waypoints}")
    print(f"完成率: {(completed_waypoints/total_waypoints*100):.1f}%" if total_waypoints > 0 else "完成率: N/A")
    
    # 航向控制性能
    print(f"\n航向控制性能:")
    print(f"  平均航向误差: {df['heading_abs_error'].mean():.2f}°")
    print(f"  最大航向误差: {df['heading_abs_error'].max():.2f}°")
    print(f"  航向误差标准差: {df['heading_abs_error'].std():.2f}°")
    print(f"  误差<5°的时间占比: {(df['heading_abs_error'] < 5).mean()*100:.1f}%")
    print(f"  误差<10°的时间占比: {(df['heading_abs_error'] < 10).mean()*100:.1f}%")
    
    # 速度控制性能
    print(f"\n速度控制性能:")
    print(f"  平均速度误差: {df['speed_abs_error'].mean():.3f} m/s")
    print(f"  最大速度误差: {df['speed_abs_error'].max():.3f} m/s")
    print(f"  速度误差标准差: {df['speed_abs_error'].std():.3f} m/s")
    print(f"  误差<0.2m/s的时间占比: {(df['speed_abs_error'] < 0.2).mean()*100:.1f}%")
    print(f"  误差<0.5m/s的时间占比: {(df['speed_abs_error'] < 0.5).mean()*100:.1f}%")
    
    # PID组件分析
    print(f"\n航向PID组件统计:")
    print(f"  P项平均值: {df['heading_p'].mean():.2f}")
    print(f"  I项平均值: {df['heading_i'].mean():.2f}")
    print(f"  D项平均值: {df['heading_d'].mean():.2f}")
    try:
        heading_total = abs(df['heading_p']).mean() + abs(df['heading_i']).mean() + abs(df['heading_d']).mean()
        if heading_total > 0:
            print(f"  P项占比: {abs(df['heading_p']).mean() / heading_total * 100:.1f}%")
    except:
        pass
    
    print(f"\n速度PID组件统计:")
    print(f"  P项平均值: {df['speed_p'].mean():.2f}")
    print(f"  I项平均值: {df['speed_i'].mean():.2f}")
    print(f"  D项平均值: {df['speed_d'].mean():.2f}")
    try:
        speed_total = abs(df['speed_p']).mean() + abs(df['speed_i']).mean() + abs(df['speed_d']).mean()
        if speed_total > 0:
            print(f"  P项占比: {abs(df['speed_p']).mean() / speed_total * 100:.1f}%")
    except:
        pass
    
    # 控制输出统计
    print(f"\n控制输出统计:")
    print(f"  平均舵角: {df['steer_l'].mean():.2f}°")
    print(f"  最大舵角: {abs(df['steer_l']).max():.2f}°")
    print(f"  平均左电机输出: {df['motor_l'].mean():.1f}")
    print(f"  平均右电机输出: {df['motor_r'].mean():.1f}")
    
    print("="*60)

def plot_correlation_analysis(df, save_path=None):
    """绘制相关性分析图"""
    fig, axes = plt.subplots(2, 2, figsize=(15, 10))
    
    # 航向误差 vs 舵角输出
    axes[0,0].scatter(df['heading_error'], df['steer_l'], alpha=0.6, s=10)
    axes[0,0].set_xlabel('航向误差 (度)')
    axes[0,0].set_ylabel('舵角输出 (度)')
    axes[0,0].set_title('航向误差 vs 舵角输出')
    axes[0,0].grid(True, alpha=0.3)
    
    # 速度误差 vs 电机输出
    axes[0,1].scatter(df['speed_error'], df['motor_l'], alpha=0.6, s=10, color='green')
    axes[0,1].set_xlabel('速度误差 (m/s)')
    axes[0,1].set_ylabel('左电机输出')
    axes[0,1].set_title('速度误差 vs 电机输出')
    axes[0,1].grid(True, alpha=0.3)
    
    # 距离目标 vs 目标速度
    axes[1,0].scatter(df['distance_to_target'], df['target_speed'], alpha=0.6, s=10, color='purple')
    axes[1,0].set_xlabel('到目标距离 (米)')
    axes[1,0].set_ylabel('目标速度 (m/s)')
    axes[1,0].set_title('距离 vs 目标速度')
    axes[1,0].grid(True, alpha=0.3)
    
    # 航向误差 vs 距离目标
    axes[1,1].scatter(df['distance_to_target'], df['heading_abs_error'], alpha=0.6, s=10, color='red')
    axes[1,1].set_xlabel('到目标距离 (米)')
    axes[1,1].set_ylabel('航向绝对误差 (度)')
    axes[1,1].set_title('距离 vs 航向误差')
    axes[1,1].grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_correlation.png", dpi=300, bbox_inches='tight')
    plt.show()

def plot_pid_tuning_analysis(df, save_path=None):
    """PID调参分析图"""
    fig, axes = plt.subplots(2, 2, figsize=(15, 10))
    
    # 航向控制响应分析
    # 找到大的航向变化点
    heading_change = df['target_heading'].diff().abs()
    large_changes = heading_change > 30  # 超过30度的变化
    
    if large_changes.any():
        change_indices = df[large_changes].index
        for idx in change_indices[:3]:  # 只显示前3个变化
            start_idx = max(0, idx - 50)
            end_idx = min(len(df), idx + 150)
            time_window = df.iloc[start_idx:end_idx]['time_sec'] - df.iloc[idx]['time_sec']
            
            axes[0,0].plot(time_window, df.iloc[start_idx:end_idx]['heading_error'], 
                          label=f'变化点{len(axes[0,0].lines)+1}', linewidth=2)
    
    axes[0,0].set_xlabel('相对时间 (秒)')
    axes[0,0].set_ylabel('航向误差 (度)')
    axes[0,0].set_title('航向阶跃响应')
    axes[0,0].legend()
    axes[0,0].grid(True, alpha=0.3)
    axes[0,0].axhline(y=0, color='k', linestyle='--', alpha=0.5)
    
    # 速度控制响应分析
    speed_change = df['target_speed'].diff().abs()
    large_speed_changes = speed_change > 0.5
    
    if large_speed_changes.any():
        change_indices = df[large_speed_changes].index
        for idx in change_indices[:3]:
            start_idx = max(0, idx - 50)
            end_idx = min(len(df), idx + 150)
            time_window = df.iloc[start_idx:end_idx]['time_sec'] - df.iloc[idx]['time_sec']
            
            axes[0,1].plot(time_window, df.iloc[start_idx:end_idx]['speed_error'], 
                          label=f'变化点{len(axes[0,1].lines)+1}', linewidth=2)
    
    axes[0,1].set_xlabel('相对时间 (秒)')
    axes[0,1].set_ylabel('速度误差 (m/s)')
    axes[0,1].set_title('速度阶跃响应')
    axes[0,1].legend()
    axes[0,1].grid(True, alpha=0.3)
    axes[0,1].axhline(y=0, color='k', linestyle='--', alpha=0.5)
    
    # 超调和振荡分析
    window_size = min(100, len(df) // 5)
    if window_size > 1:
        # 计算移动窗口内的最大最小值差异（反映振荡程度）
        df['heading_oscillation'] = df['heading_error'].rolling(window=window_size).apply(lambda x: x.max() - x.min())
        df['speed_oscillation'] = df['speed_error'].rolling(window=window_size).apply(lambda x: x.max() - x.min())
        
        axes[1,0].plot(df['time_sec'], df['heading_oscillation'], 'r-', linewidth=2)
        axes[1,0].set_xlabel('时间 (秒)')
        axes[1,0].set_ylabel('航向振荡幅度 (度)')
        axes[1,0].set_title('航向控制振荡分析')
        axes[1,0].grid(True, alpha=0.3)
        
        axes[1,1].plot(df['time_sec'], df['speed_oscillation'], 'g-', linewidth=2)
        axes[1,1].set_xlabel('时间 (秒)')
        axes[1,1].set_ylabel('速度振荡幅度 (m/s)')
        axes[1,1].set_title('速度控制振荡分析')
        axes[1,1].grid(True, alpha=0.3)
    
    plt.tight_layout()
    if save_path:
        plt.savefig(f"{save_path}_tuning_analysis.png", dpi=300, bbox_inches='tight')
    plt.show()

def generate_tuning_recommendations(df):
    """生成PID调参建议"""
    print("\n" + "="*60)
    print("PID调参建议")
    print("="*60)
    
    # 航向控制分析
    avg_heading_error = df['heading_abs_error'].mean()
    max_heading_error = df['heading_abs_error'].max()
    heading_std = df['heading_abs_error'].std()
    
    print("航向控制分析:")
    if avg_heading_error > 10:
        print("  ⚠️  平均航向误差较大，建议:")
        print("    - 增加比例增益(Kp)")
        print("    - 检查执行器响应是否足够")
    elif avg_heading_error < 2:
        print("  ✅ 平均航向误差较小，控制精度良好")
    
    if heading_std > 5:
        print("  ⚠️  航向误差波动较大，建议:")
        print("    - 减小微分增益(Kd)以减少噪声敏感性")
        print("    - 增加滤波或减小控制频率")
    
    if max_heading_error > 30:
        print("  ⚠️  最大航向误差过大，建议:")
        print("    - 检查是否存在执行器饱和")
        print("    - 适当增加比例增益")
    
    # 检查积分饱和
    heading_i_max = df['heading_i'].max()
    heading_i_min = df['heading_i'].min()
    if heading_i_max > 1000 or heading_i_min < -1000:
        print("  ⚠️  积分项可能饱和，建议:")
        print("    - 减小积分增益(Ki)")
        print("    - 设置积分限幅")
    
    print("\n速度控制分析:")
    avg_speed_error = df['speed_abs_error'].mean()
    max_speed_error = df['speed_abs_error'].max()
    speed_std = df['speed_abs_error'].std()
    
    if avg_speed_error > 0.5:
        print("  ⚠️  平均速度误差较大，建议:")
        print("    - 增加比例增益(Kp)")
        print("    - 检查推进系统是否正常")
    elif avg_speed_error < 0.1:
        print("  ✅ 平均速度误差较小，控制精度良好")
    
    if speed_std > 0.3:
        print("  ⚠️  速度误差波动较大，建议:")
        print("    - 增加积分增益(Ki)以消除稳态误差")
        print("    - 适当调整微分增益(Kd)")
    
    # 检查超调
    if 'speed_oscillation' in df.columns:
        avg_oscillation = df['speed_oscillation'].mean()
        if avg_oscillation > 1.0:
            print("  ⚠️  速度控制存在振荡，建议:")
            print("    - 减小比例增益(Kp)")
            print("    - 减小微分增益(Kd)")
            print("    - 增加系统阻尼")
    
    print("\n总体建议:")
    
    # 基于P项占比给出建议
    try:
        heading_p_ratio = abs(df['heading_p']).mean() / (abs(df['heading_p']).mean() + abs(df['heading_i']).mean() + abs(df['heading_d']).mean())
        speed_p_ratio = abs(df['speed_p']).mean() / (abs(df['speed_p']).mean() + abs(df['speed_i']).mean() + abs(df['speed_d']).mean())
        
        if heading_p_ratio > 0.8:
            print("  - 航向控制主要依赖P项，可考虑增加I项和D项的作用")
        if speed_p_ratio > 0.8:
            print("  - 速度控制主要依赖P项，可考虑增加I项以消除稳态误差")
            
    except:
        pass
    
    print("  - 建议进行参数扫描实验以找到最优参数组合")
    print("  - 在不同海况和负载条件下验证参数鲁棒性")
    print("="*60)

def main():
    """主函数"""
    # 声明使用全局变量
    global DEFAULT_CSV_PATH

    parser = argparse.ArgumentParser(
        description='USV PID调试数据分析工具',
        epilog="""
使用示例:
  python3 plot.py                           # 自动分析最新的CSV文件
  python3 plot.py usv_pid_debug.csv        # 分析指定的CSV文件
  python3 plot.py usv_pid_debug             # 分析匹配该名称的最新文件
  python3 plot.py --list                   # 列出所有可用的CSV文件
  python3 plot.py file.csv -o results      # 分析并保存图片
  python3 plot.py file.csv --no-display    # 只保存图片不显示
        """,
        formatter_class=argparse.RawDescriptionHelpFormatter
    )
    
    parser.add_argument('csv_file', nargs='?', help='CSV数据文件路径或文件名（可选）')
    parser.add_argument('--output', '-o', help='输出图片文件前缀', default=None)
    parser.add_argument('--no-display', action='store_true', help='不显示图片，只保存')
    parser.add_argument('--list', '-l', action='store_true', help='列出所有可用的CSV文件')
    parser.add_argument('--path', '-p', default=DEFAULT_CSV_PATH, help=f'CSV文件搜索路径 (默认: {DEFAULT_CSV_PATH})')
    
    args = parser.parse_args()
    
    # 更新默认搜索路径
    DEFAULT_CSV_PATH = args.path
    
    # 如果只是列出文件
    if args.list:
        list_available_files(DEFAULT_CSV_PATH)
        return
    
    # 确定要分析的CSV文件
    csv_file_path = get_csv_file_path(args)
    if not csv_file_path:
        return
    
    # 设置输出路径
    if args.output:
        save_path = args.output
    else:
        base_name = os.path.splitext(os.path.basename(csv_file_path))[0]
        timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
        save_path = f"analysis_{base_name}_{timestamp}"
    
    # 如果不显示图片，设置matplotlib后端
    if args.no_display:
        import matplotlib
        matplotlib.use('Agg')
        print("设置为无显示模式，图片将保存到文件")
    
    # 加载数据
    df = load_and_process_data(csv_file_path)
    if df is None:
        return
    
    # 生成统计报告
    generate_statistics_report(df)
    
    # 生成调参建议
    generate_tuning_recommendations(df)
    
    # 绘制分析图
    print(f"\n正在生成分析图表...")
    
    try:
        plot_trajectory(df, save_path if args.output else None)
        plot_heading_control(df, save_path if args.output else None)
        plot_speed_control(df, save_path if args.output else None)
        plot_pid_performance_summary(df, save_path if args.output else None)
        plot_correlation_analysis(df, save_path if args.output else None)
        plot_pid_tuning_analysis(df, save_path if args.output else None)
        
        print(f"✅ 分析完成！")
        if args.output:
            print(f"📊 图片已保存，前缀: {save_path}")
        if args.no_display:
            print(f"💾 所有图片已保存到文件，未显示图表")
            
    except Exception as e:
        print(f"❌ 生成图表时出错: {e}")
        print("请检查数据完整性和依赖包安装情况")

if __name__ == "__main__":
    main()